% streckenübertragungsfunktion
s = tf('s');
% stabil (hurwitzpolynom, 2 grad, selbes vorzeichen und reell kleiner 0)
zG = (800*(1+s/6));
nG = (s^2*(1+s/3)*(1+0.005363258140*s+1/10000*s^2));
G = zG/nG
%bode(G)

% anforderungen
% doppelintegratorvorgabe (keine bleib. regelabweichung auf rampenf.
% referenzsignal) e_inf|d(t)=t = 0
% doppelintegrator bereits vorhanden
di = 1;
tr = 0.1; % anstiegszeit
ue = 30; % erlaubtes überschwingen

% durchtrittsfrequenz 20*log(abs(L(I*omega))) = 0 => abs(L(I*omega)) = 1
% und wc*tr approx 1.5 => 1.5/tr = wc
wc = 1.5/tr;
% phasenreserve phi [grad] + ue [%] approx 70 => phi [grad] = 70 - ue [%]
% = 70 - 100 * ue
% phi [rad] = phi [grad] / 180 * pi
phi = 70-ue;

R1 = di*(1+0.005363258140*s+1/10000*s^2)/(1+s/6)*(1+s/3);
L1 = R1*G;
figure(1);
bode(L1);
grid on;

% Betrags- und Phasenkorrektur
L1_wc = freqresp(L1, wc);
L1_phig = angle(L1_wc)*180/pi
dphi = -(180-phi)*pi/180-(angle(L1_wc));
dphig = dphi*180/pi

Tr1 = tan(80/180*pi)/wc;
Tr2 = tan((40/180*pi)/3)/wc;

R2 = (1+s*Tr1)/((1+s*Tr2)^3);
L2 = R2*L1;
L2_wc = freqresp(L2, wc);
Vr = 1/abs(L2_wc);

% Bodediagramm des offenen Regelkreises
L = Vr*L2;
figure(2);
bode(L1, L2, L);
legend('L1', 'L2', 'L');
grid on;

% Test, dass Betrag und Phase an der Durchtrittsfrequenz korrekt sind 
disp(['Phase bei Durchtrittsfrequenz: ', num2str(angle(freqresp(L,wc))*180/pi)]);
disp(['Amplitude bei Durchtrittsfrequenz: ', num2str(abs(freqresp(L,wc)))]);

% Sprungantwort des geschlossenen Regelkreises
T = feedback(L,1);
figure(3);
step(T,5);
grid on;

R = minreal(Vr*R1*R2)

% Sprungantwort der Stellgrößenübertragungsfunktion
Tu = R/(1+R*G);
figure(4);
step(100*Tu,3);
grid on;